#include "stair_ctrl/kinematics.hpp"
#include <vector>
#include <iostream>

int main()
{
    float offset_x = 0.1881;
    float offset_y = 0.04675;
    float l_abad = 0.08;
    float l_shank = 0.213;
    float l_thigh = 0.213;

    Eigen::Vector3f rho_opt;
    rho_opt.setZero();
    Eigen::VectorXf rho_fl, rho_fr, rho_hl, rho_hr;
    rho_fl.setZero(5);
    rho_fl << offset_x, offset_y, l_abad, l_shank, l_thigh;
    rho_fr.setZero(5);
    rho_fr << offset_x, -offset_y, -l_abad, l_shank, l_thigh;   
    rho_hl.setZero(5);
    rho_hl << -offset_x, offset_y, l_abad, l_shank, l_thigh;   
    rho_hr.setZero(5);
    rho_hr << -offset_x, -offset_y, -l_abad, l_shank, l_thigh;

    std::vector<Eigen::VectorXf> test_rho = {rho_fl, rho_fr, rho_hl, rho_hr};

    std::vector<Eigen::Matrix<float, 3, 1>> test_q;
    test_q.push_back(Eigen::Matrix<float, 3, 1>(0.0, 0.6, -1.2));
    test_q.push_back(Eigen::Matrix<float, 3, 1>(-1.57, 0.6, -1.2));
    test_q.push_back(Eigen::Matrix<float, 3, 1>(1.57, 0.6, -1.2));
    test_q.push_back(Eigen::Matrix<float, 3, 1>(0.0, 1.57, -1.57));
    
    auto kin = kinematics<float>();

    for (size_t i = 0; i < test_q.size(); ++i)
    {
        std::cout << "test_q: " << test_q.at(i).transpose() << std::endl
                    << "rho: " << test_rho.at(i).transpose() << std::endl
                    << "output: " << kin.fk(test_q.at(i), rho_opt, test_rho.at(i)).transpose() << std::endl
                    << std::endl;
    }
}